﻿using Box2DX.Collision;
using Box2DX.Common;
/* ***************************************************************************
 *  This file is part of the C# port of the Invivo AI challenge simulation environment.
 *  Copyright 2010 Colin Green
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program .  If not, see <http://www.gnu.org/licenses/>.
 * 
 * 
 * This file incorporates open sourced work authored by the following people:
 *
 *   Copyright (c) 2009 Simon Funk
 *   Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
 *   Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com*   
 */
/*
  Box2DX Copyright (c) 2008 Ihar Kalasouski http://code.google.com/p/box2dx
  Box2D original C++ version Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
*/
using Box2DX.Dynamics;
using Tao.OpenGl;

namespace FunkAIChallenge
{
    /// <summary>
    /// The Funk AI challenge world constructed as a box2d world.
    /// </summary>
    public class SimulationWorld
    {
        #region Constants [Simulation defaults]

        const float __defaultFriction = 0.2f;
        const float __defaultRestitution = 0.0f;
        const float __defaultDensity = 1.0f;
        
        #endregion

        #region Instance Fields

        readonly SimulationParameters _simParams;
        readonly World _world;

        float _currentTime = 0f;
        Body _ballBody;
        Player _player;

        WorldContactListener _contactListener;

        /// <summary>
        /// For debug/testing purposes. The mousejoint is created on a mouseclick and allows world objects to be grabbed and moved around.
        /// The joint is destroyed when the mouseclick is released (mouseup event).
        /// </summary>
        MouseJoint _mouseJoint;

        #endregion

        #region Constructors

        /// <summary>
        /// Construct with default simulation parameters.
        /// </summary>
        public SimulationWorld() : this(new SimulationParameters())
        {
        }

        /// <summary>
        /// Construct with provided simulation parameters.
        /// </summary>
        public SimulationWorld(SimulationParameters simParams)
        {
            _simParams = simParams;
            _world = CreateBox2DWorld();

            // Allow physics calcs to use values from previous timestep.
            _world.SetWarmStarting(true);

            // Enable additional collision detection for high speed objects (that might not ever contact each other at a given timestep due to speed).
            _world.SetContinuousPhysics(true);

            // Put stuff in the world.
            PopulateWorld(_world);

            // Create contact listener and register it with world.
            _contactListener = new WorldContactListener(_ballBody, _player.HandBody);
            _world.SetContactListener(_contactListener);
        }

        #endregion

        #region Properties

        /// <summary>
        /// Gets the current simulation time (seconds).
        /// </summary>
        public float CurrentTime
        {
            get { return _currentTime; }
        }

        /// <summary>
        /// Gets a flag that indicates if there has been contact between hand and ball in the last timestep.
        /// </summary>
        public bool HandBallContact
        {
            get { return _contactListener.HandBallContact; }
        }

        #endregion

        #region Public Methods

        /// <summary>
        /// Create an interface onto the single player.
        /// </summary>
        /// <returns></returns>
        public PlayerInterface CreatePlayerInterface()
        {
            return new PlayerInterface(this, _ballBody, _player, new float[]{4f, 2f, 1f});
        }

        /// <summary>
        /// Sets a drawing routine. Methods on the provided object are called-back to perform drawing on each update.
        /// </summary>
        /// <param name="debugDraw"></param>
        public void SetDebugDraw(DebugDraw debugDraw)
        {
            _world.SetDebugDraw(debugDraw);
        }

        /// <summary>
        /// Perform one simulation timestep; move the simulation forward by the timestep increment duration.
        /// </summary>
        public void Step()
        {
            _contactListener.Reset();
			_world.Step(_simParams._timeStep, _simParams._velocityIters, _simParams._positionIters);
			_world.Validate();
            _currentTime += _simParams._timeStep;

            // Draw mouse joint if present.
            if (_mouseJoint != null)
            {
                Body body = _mouseJoint.GetBody2();
                Vec2 p1 = body.GetWorldPoint(_mouseJoint._localAnchor);
                Vec2 p2 = _mouseJoint._target;

                Gl.glPointSize(4.0f);
                Gl.glColor3f(0.0f, 1.0f, 0.0f);
                Gl.glBegin(Gl.GL_POINTS);
                Gl.glVertex2f(p1.X, p1.Y);
                Gl.glVertex2f(p2.X, p2.Y);
                Gl.glEnd();
                Gl.glPointSize(1.0f);

                Gl.glColor3f(0.8f, 0.8f, 0.8f);
                Gl.glBegin(Gl.GL_LINES);
                Gl.glVertex2f(p1.X, p1.Y);
                Gl.glVertex2f(p2.X, p2.Y);
                Gl.glEnd();
            }
        }

        #endregion

        #region Public Methods [External Event Handling]

        /// <summary>
        /// Mouse interaction event.
        /// </summary>
		public void MouseDown(Vec2 p)
		{
			if (_mouseJoint != null)
			{
				return;
			}

			// Make a small box.
			AABB aabb = new AABB();
			Vec2 d = new Vec2();
			d.Set(0.001f, 0.001f);
			aabb.LowerBound = p - d;
			aabb.UpperBound = p + d;

			// Query the world for overlapping shapes.
			int k_maxCount = 10;
			Shape[] shapes = new Shape[k_maxCount];
			int count = _world.Query(aabb, shapes, k_maxCount);
			Body body = null;
			for (int i = 0; i < count; ++i)
			{
				Body shapeBody = shapes[i].GetBody();
				if (shapeBody.IsStatic() == false && shapeBody.GetMass() > 0.0f)
				{
					bool inside = shapes[i].TestPoint(shapeBody.GetXForm(), p);
					if (inside)
					{
						body = shapes[i].GetBody();
						break;
					}
				}
			}

			if (body != null)
			{
				MouseJointDef md = new MouseJointDef();
				md.Body1 = _world.GetGroundBody();
				md.Body2 = body;
				md.Target = p;

                md.MaxForce = 1000.0f * body.GetMass();

				_mouseJoint = (MouseJoint)_world.CreateJoint(md);
				body.WakeUp();
			}
		}

        /// <summary>
        /// Mouse interaction event.
        /// </summary>
		public void MouseUp()
		{
			if (_mouseJoint != null)
			{
				_world.DestroyJoint(_mouseJoint);
				_mouseJoint = null;
			}
		}

        /// <summary>
        /// Mouse interaction event.
        /// </summary>
		public void MouseMove(Vec2 p)
		{
			if (_mouseJoint != null)
			{
				_mouseJoint.SetTarget(p);
			}
		}

        #endregion

        #region Private Methods [World Creation/Init]

        private World CreateBox2DWorld()
        {
			AABB worldAABB = new AABB();
			worldAABB.LowerBound.Set(_simParams._lowerBound.X, _simParams._lowerBound.Y);
			worldAABB.UpperBound.Set(_simParams._upperBound.X, _simParams._upperBound.Y);
			Vec2 gravity = new Vec2();
			gravity.Set(0.0f, _simParams._gravity);
			bool doSleep = false;
			World world = new World(worldAABB, gravity, doSleep);
            return world;
        }

        private void PopulateWorld(World world)
        {
		// ==== Define the ground body ====
			BodyDef groundBodyDef = new BodyDef();
			groundBodyDef.Position.Set(0f, -0.25f);
            
   			// Call the body factory which creates the ground box shape.
			// The body is also added to the world.
			Body groundBody = world.CreateBody(groundBodyDef);

			// Define the ground box shape.
			PolygonDef groundShapeDef = new PolygonDef();

			// The extents are the half-widths of the box.
			groundShapeDef.SetAsBox(10.5f, 0.25f);
            groundShapeDef.Friction =__defaultFriction;
            groundShapeDef.Restitution = __defaultRestitution;
            groundShapeDef.Filter.CategoryBits = 0x3;
            
			// Add the ground shape to the ground body.
            groundBody.CreateShape(groundShapeDef);
            
        // ==== Left wall ====
			BodyDef wallBodyDef = new BodyDef();
			wallBodyDef.Position.Set(-10.25f, 5f);
            Body leftWallBody = world.CreateBody(wallBodyDef);
            PolygonDef wallShapeDef = new PolygonDef();
            wallShapeDef.SetAsBox(0.25f,5.5f);
            wallShapeDef.Friction = __defaultFriction;
            wallShapeDef.Restitution = __defaultRestitution;
            leftWallBody.CreateShape(wallShapeDef);

        // ==== Right wall ====
			wallBodyDef.Position.Set(10.25f, 5f);
            Body rightWallBody = world.CreateBody(wallBodyDef);
            rightWallBody.CreateShape(wallShapeDef);

        // ==== Sky ====
			BodyDef skyBodyDef = new BodyDef();
			skyBodyDef.Position.Set(0f, 10.25f);
            Body skyBody = world.CreateBody(skyBodyDef);
            PolygonDef skyShapeDef = new PolygonDef();
            skyShapeDef.SetAsBox(10.5f,0.25f);
            skyShapeDef.Friction = __defaultFriction;
            skyShapeDef.Restitution = __defaultRestitution;
            skyBody.CreateShape(skyShapeDef);

        // ==== Mound ====
            BodyDef moundBodyDef = new BodyDef();
            moundBodyDef.Position.Set(0f, 0f);

            // Mound body.
            Body moundBody = world.CreateBody(moundBodyDef);

            // Define shape.
            PolygonDef moundShapeDef = new PolygonDef();
            moundShapeDef.VertexCount = 3;
            moundShapeDef.Vertices[0].Set(7f, 0f);
            moundShapeDef.Vertices[1].Set(0f, 0.5f);
            moundShapeDef.Vertices[2].Set(-7f, 0f);
            moundShapeDef.Friction = __defaultFriction;
            moundShapeDef.Restitution = __defaultRestitution;

            // Add shape to body.
            moundBody.CreateShape(moundShapeDef);

        // ==== Create player arm/body ====
            _player = CreatePlayerBody(-8f, 0f);

        // ==== Create Ball ====
            _ballBody = CreateBall();
        }

        private Body CreateBall()
        {
			BodyDef bd = new BodyDef();
			bd.AllowSleep = false;
			bd.Position = _simParams._ballPos;
			bd.IsBullet = true;
			Body body = _world.CreateBody(bd);
			body.SetLinearVelocity(_simParams._ballVelocity);

			CircleDef shapeDef = new CircleDef();
			shapeDef.Radius = 0.1f;
			shapeDef.Density = __defaultDensity;
			shapeDef.Restitution = 0.9f;
            shapeDef.Filter.MaskBits = 0x1;
			body.CreateShape(shapeDef);
			body.SetMassFromShapes();
            return body;
        }

        private Player CreatePlayerBody(float x, float y)
        {
            const float rodRadius = 0.05f;	// Half the thickness of the rods.
            const float density = 0.2f;

            // Joint limit angles.
            float a1 = (float)(3.0*System.Math.PI/8.0);
            float a2 = (float)(11.0*System.Math.PI/8.0);

            // Baseline joint def (re-used).
			RevoluteJointDef jointDef = new RevoluteJointDef();

            // Prevent collision detection between connected arms (that would bad given that they overlap aroudn the hinge point).
            jointDef.CollideConnected = false;
            jointDef.EnableMotor = true;
            jointDef.MaxMotorTorque = 0f;
            
            // Player base.
            BodyDef bodyDef = new BodyDef();
            bodyDef.Position.Set(x, y+0.125f);

            Body baseBody = _world.CreateBody(bodyDef);
            PolygonDef baseShapeDef = new PolygonDef();
            baseShapeDef.SetAsBox(0.25f, 0.125f);
            baseShapeDef.Friction = __defaultFriction;
            baseShapeDef.Restitution = __defaultRestitution;
            baseBody.CreateShape(baseShapeDef);

            // Arm segment 1.
            Vec2 armBasePos = new Vec2(x, y+0.25f);
            Body arm1Body = CreateBarVertical(armBasePos, new Vec2(x, y+1.25f), rodRadius, __defaultFriction, __defaultRestitution, density, false, 0x2);

            // Join arm1 to base.
            jointDef.Initialize(baseBody, arm1Body, new Vec2(x, y+0.25f));
            RevoluteJoint joint1 = (RevoluteJoint)_world.CreateJoint(jointDef);

            // Arm segment 2.
            Body arm2Body = CreateBarHorizontal(new Vec2(x, y+1.25f), new Vec2(x+1f, y+1.25f), rodRadius, __defaultFriction, __defaultRestitution, density, false, 0x2);

            // Join arm2 to arm1.
            jointDef.Initialize(arm1Body, arm2Body, new Vec2(x, y+1.25f));
            jointDef.LowerAngle = -a1;
            jointDef.UpperAngle = a2;
            jointDef.EnableLimit = true;
            RevoluteJoint joint2 = (RevoluteJoint)_world.CreateJoint(jointDef);

            // Hand.
            Body handBody = CreateBarVertical(new Vec2(x+1f, y+1.25f), new Vec2(x+1f, y+1.75f), rodRadius, __defaultFriction, __defaultRestitution, __defaultDensity, false, 0xff);
            handBody.SetBullet(true);    // Otherwise parts of the hand (e.g. circle endpoints) could pass through the ground if the arm is moving fast enough.        

            // Join hand to arm2.
            jointDef.Initialize(arm2Body, handBody, new Vec2(x+1f, y+1.25f));
            jointDef.LowerAngle = -a2;
            jointDef.UpperAngle = a1;
            jointDef.EnableLimit = true;
            RevoluteJoint joint3 = (RevoluteJoint)_world.CreateJoint(jointDef);

            return new Player(bodyDef.Position, armBasePos, handBody, new RevoluteJoint[]{joint1, joint2, joint3});
        }

        private Body CreateBarVertical(Vec2 endA, Vec2 endB, float radius, float friction, float restitution, float density, bool isSensor, ushort layers)
        {
            // ==== Create bar. ====
            Vec2 centerPos = (endA+endB) * 0.5f;
            float halfLength = (endB.Y - endA.Y) * 0.5f;

            BodyDef bodyDef = new BodyDef();
            bodyDef.Position.Set(centerPos.X, centerPos.Y);

            // Create body object; The body is also added to the world.
            Body body = _world.CreateBody(bodyDef);
            PolygonDef shapeDef = new PolygonDef();
            shapeDef.SetAsBox(radius, halfLength);
            shapeDef.Friction = friction;
            shapeDef.Restitution = restitution;
            shapeDef.Density = density;
            shapeDef.IsSensor = isSensor;
            shapeDef.Filter.MaskBits = layers;
            shapeDef.Filter.CategoryBits = 0x3;

            body.CreateShape(shapeDef);
        
            // ==== Place some end caps on the bar. ====
            CircleDef circleDef = new CircleDef();
            circleDef.Radius = radius;
            circleDef.Friction = friction;
            circleDef.Restitution = restitution;
            circleDef.Density = density;
            circleDef.IsSensor = isSensor;
            circleDef.Filter.MaskBits = layers;
            circleDef.Filter.CategoryBits = 0x3;

            circleDef.LocalPosition = endA-centerPos;
            body.CreateShape(circleDef);
            
            circleDef.LocalPosition = endB-centerPos;
            body.CreateShape(circleDef);
            body.SetMassFromShapes();

            return body;
        }

        private Body CreateBarHorizontal(Vec2 endA, Vec2 endB, float radius, float friction, float restitution, float density, bool isSensor, ushort layers)
        {
            // ==== Create bar. ====
            Vec2 centerPos = (endA+endB) * 0.5f;
            float halfLength = (endB.X - endA.X) * 0.5f;

            BodyDef bodyDef = new BodyDef();
            bodyDef.Position.Set(centerPos.X, centerPos.Y);

            // Create body object; The body is also added to the world.
            Body body = _world.CreateBody(bodyDef);
            PolygonDef shapeDef = new PolygonDef();
            shapeDef.SetAsBox(halfLength, radius);
            shapeDef.Friction = friction;
            shapeDef.Restitution = restitution;
            shapeDef.Density = density;
            shapeDef.IsSensor = isSensor;
            shapeDef.Filter.MaskBits = layers;
            shapeDef.Filter.CategoryBits = 0x3;
            body.CreateShape(shapeDef);

            // ==== Place some end caps on the bar. ====
            CircleDef circleDef = new CircleDef();
            circleDef.Radius = radius;
            circleDef.Friction = friction;
            circleDef.Restitution = restitution;
            circleDef.Density = density;
            circleDef.IsSensor = isSensor;
            circleDef.Filter.MaskBits = layers;
            circleDef.Filter.CategoryBits = 0x3;

            circleDef.LocalPosition = endA-centerPos;
            body.CreateShape(circleDef);
            
            circleDef.LocalPosition = endB-centerPos;
            body.CreateShape(circleDef);
            body.SetMassFromShapes();

            return body;
        }

        #endregion
    }
}
